"""Several matrix utilities."""

import vector
from math import *

def cofactor(m,irow,icol):
    """Returns the cofactor icol,irow of matrix m."""
    def minor():
        """Returns a minor matrix of m, where row irow and col icol were removed."""
        return [row[:icol]+row[icol+1:] for row in m[:irow]+m[irow+1:]]
    if (irow+icol)%2==1: 
        return -determinant(minor())
    else:
        return determinant(minor())

def adjugate (m):
    """Returns the adjugate of m, i.e., a matrix of all cofactors of m."""
    n = len (m)
    assert (n == len(m[0]))
    return [[cofactor(m,irow,icol) for irow in range(n)] for icol in range(n)]
            
def determinant(m):
    """Computes the determinant of a matrix by its cofactor expansion."""
    n = len (m)
    assert (n == len(m[0]))
    if n==2: 
        a,b=m[0]
        c,d=m[1]
        return a*d-b*c
    else:
        return sum ([m[0][i]*cofactor(m,0,i) for i in range(n)])

def transpose(m):
    """Returns the transpose of m."""    
    return [column(m,i) for i in range(len(m[0]))]

def column(m,i):
    """Returns the column i of matrix m."""
    return [row[i] for row in m]
    
def product(m,l):
    """Returns the product of two matrices."""
    return [[vector.dot(row,column(l,j)) for j in range(len(l[0]))] for row in m]

def scalarproduct (m, s):
    """Returns m where all elements were multiplied by scalar s."""
    return [[x*s for x in row] for row in m]

def add(m1,m2):
    """Returns the sum of two matrices m1 and m2, which must have the same size."""
    assert(len(m1)==len(m2) and len(m1[0])==len(m2[0]))
    return [[a+b for a,b in zip(row1,row2)] for row1,row2 in zip(m1,m2)]

def sub(m1,m2):
    """Returns the difference of two matrices m1 and m2, which must have the same size."""
    assert(len(m1)==len(m2) and len(m1[0])==len(m2[0]))
    return [[a-b for a,b in zip(row1,row2)] for row1,row2 in zip(m1,m2)]
    
def inverse(m):
    """Computes the inverse of m by cofactor expansion."""
    det = 1.0/determinant(m)
    return [[det*x for x in row] for row in adjugate(m)]
    
def identity(n):
    """Returns a square identity matrix of size n x n."""
    return [[0]*i+[1]+[0]*(n-i-1) for i in range(n)]
    
#===============================================================================
# Below, specific code for 4x4 matrices
#===============================================================================

def transform(m,v):
    """Returns the product of matrix m by column vector v. 
    v is assumed to be in homogeneous coordinates."""
    assert (len(m[0])==len(v))
    return [vector.dot(row,v) for row in m]

def transformvector(m,v):
    """Just like transform, but v is assumed to be of length 3."""
    return transform (m, [v[0],v[1],v[2],0])[:3]

def transformpoint(m,p):
    """Transforms point p by matrix m, where p is assumed to be of length 3."""
    return transform (m, [p[0],p[1],p[2],1])[:3]
    
def translation(*v):
    """Returns a 4x4 matrix corresponding to translation by vector v.
    If v has only 2 coordinates, z=0 is assumed. If v has 4 coordinates,
    the homogenous (fourth) coordinate must be 0."""
    m = identity(4)
    for i in range(min(len(v),3)): m [i][3] = v[i]
    assert(len(v)<4 or len(v)==4 and v[3]==0)
    return m

def scale(*v):
    """Returns a 4x4 matrix corresponding to scale by vector v.
    If v has only 2 coordinates, z=1 is assumed. If v has 4 coordinates,
    the homogenous (fourth) coordinate must be 0."""
    m = identity(4)
    for i in range(min(len(v),3)): m [i][i] = v[i]
    assert(len(v)<4 or len(v)==4 and v[3]==0)
    return m

def rotation(angle,*v):
    """Returns a 4x4 matrix corresponding to a rotation of angle radians about 
    axis v. If v has 4 coordinates, the homogenous (fourth) coordinate must 
    be 0."""
    x,y,z = vector.normalize(v[:3])
    u = [[x],[y],[z]]
    ut = [[x,y,z]]
    s = [[0,-z,y],[z,0,-x],[-y,x,0]]
    uut = product(u,ut)
    sina,cosa = sin(angle),cos(angle)
    m = add(uut, add(scalarproduct(sub(identity(3),uut),cosa),scalarproduct(s,sina)))
    return [[m[0][0],m[0][1],m[0][2],0],
            [m[1][0],m[1][1],m[1][2],0],
            [m[2][0],m[2][1],m[2][2],0],
            [0,0,0,1]]

def rotby2vectors (startvector, endvector):
    """Given two vectors returns the rotation matrix that maps the first onto 
    the second. Both vectors must be of length 3 (cartesian) or 4 (homogeneous).
    @param startvector: original vector.
    @param endvector: destination vector.
    @return: rotation matrix.
    """
    r = vector.cross(vector.normalize(startvector), vector.normalize(endvector))
    l = vector.length (r)
    if l<1e-10: 
        angle,axis = 0,(0,0,1)
    else:
        angle,axis =  asin(l), vector.scale(r,1.0/l)
    return rotation (angle,*axis)

#===============================================================================
# Below, specific code for 4x4 matrices as returned by opengl "get" functions,
# i.e., stored by column, rather than by row
#===============================================================================

def gltransformpoint (m,p):
    """Returns a transformed point.
    @param m: an opengl transformation matrix, a 4x4 matrix stored by column.
    @param p: a point (3-float array).
    @return: p transformed by m.
    """
    newp = list(m [3][0:3])
    for i in range(3):
        for j in range (3):
            newp [i] = newp [i] + p[j] * m [j][i]
    return newp

def gltransformvector (m,p):
    """Returns a transformed vector.
    @param m: an opengl transformation matrix, a 4x4 matrix stored by column.
    @param p: a vector (3-float array).
    @return: p transformed by m.
    """
    newp = [0,0,0]
    for i in range(3):
        for j in range (3):
            newp [i] = newp [i] + p[j] * m [j][i]
    return newp

def glinverse (m):
    """Returns the inverse of an opengl transformation matrix. This is 
    slightly different from the inverse function because opengl's matrices
    are not regular lists.
    @param m: an opengl transformation matrix, a 4x4 matrix stored by column.
    @return: the inverse of m.
    """
    return inverse ([[x for x in row] for row in m])
